// modifs for next version: 
// (1) put thresholds here (global variables, easily changeable)
// (2) put limits of mirror excursion here too, easily changeable
// (3) add mode test without mirrors, using processing code

#include "LivingSpot.h"
#include "SpiMCP4922.h"

//int greenLaserPin=22;
//int redLaserPin = 2;

#define pinSwitch 2 // to change modes...

#define pinAnalogPot 1 //potentiometer to set the fixed threshold (readout each time we change mode with button). 
int potValue;
int potFixThreshold;


//#define SCHEDULERMODETIME 1200000//40000// 200000 // in milliseconds (REM: VERY LONG FOR SEKAI ICHI UKETAI JYUGYOU! (1200000=20 minutes...)
unsigned long SCHEDULERMODETIME = 5000;//modif Alexis time change for each mode
unsigned long schedulerTime;
int schedulerMode;

/// ALWAYS instantiate 4 SPOTS (but they don't need to be "alive" at the same time)
/// midi and scheduler functions are using 4 spots
#define NBSPOT 3
LivingSpot spotArray[NBSPOT];

boolean sendSerial=false; // this is for tests without the mirrors using the mirrorSimulator.pde on processing, or for use of any other software for generating sound (not midi based)
 
void setup(){
  setupPWM(); //setup PWM timers for the laser frequency and the lockIn phase shift

  pinMode(pinSwitch, INPUT);
  digitalWrite(pinSwitch,HIGH);// to activate pullup resistors (i.e., when switch is off, the pin reads HIGH). 

 // Potentiometer:
// ATTN: normally there is no need to set the inputs as INPUTS, unless there were configured otherwise 
  //("The analogRead command will not work correctly if a pin has been previously set to an output,"
   // For security, we do it here (actually, wihout doing this, I had a problem!). BEWARE: we need to use A0, A1, A2, not 0, 1 and 2..
  pinMode(A1, INPUT); // corresponds to analog input pin 1 (pinAnalogPot)
  digitalWrite(A1, LOW); // reset pullup resistors

  pinMode(greenLaserPin, OUTPUT);
  //digitalWrite(greenLaserPin, HIGH);
  Spi.writeOutR(4000);
  
  midiInit();
  schedulerInit();
 
  // setup serial communication with the computer (for tests):
  Serial.begin(115200);
  sendSerial=false;
 
}


// For tests angle:


/*
void loop(){
// mesure angle:
  // spotArray[0].measureAngle();

// measure limits:
        Spi.writeOutR(0);
        spotArray[0].drawMaxArea(1);
        digitalWrite(greenLaserPin, LOW);
        spotArray[0].drawMaxArea(10);
}
*/


void loop(){
  
  // pin mode for Sekaiichi: 
  if (digitalRead(pinSwitch)==LOW) {
    delay(200); // simple way to avoid switch bouncing
    schedulerTime = millis()-SCHEDULERMODETIME; // this will force the scheduler to change the mode
   // Serial.println("button pressed");
  }
  

  // for tests:
   if(schedulerMode==0) {
          Spi.writeOutR(0);
          spotArray[0].drawMaxArea(1);
          digitalWrite(greenLaserPin, LOW);
          spotArray[0].drawMaxArea(1);
          digitalWrite(greenLaserPin, HIGH);
          Spi.writeOutR(4000);
        }
 
  
  for(int i=0; i<NBSPOT; i++){
    if(spotArray[i].alive){
      
      // update threshold?
       if(spotArray[i].behaviourMode == SNELL_LAW){
       //continuous polling of threshold from potentiomenter:
       potentiometerThresholdRead();
       spotArray[i].updateThreshold(potFixThreshold);
       }
      
      // update dynamics:
      spotArray[i].update(spotArray, NBSPOT);
      
     // change radius:
    if(spotArray[i].behaviourMode == SIMPLE_BOUNCING){ 
        byte radius = 5*sin(200 * (millis()-schedulerTime) / 15000.0+3.1415*i)+18;
        spotArray[i].setRadiusSaccade( radius );
      }
      
      
      if(spotArray[i].touchWall){  
       //send midi signal: 
         midiTouchWall(i);  
        //midiCollisionPercussion(i);
      }
      
      
      if(spotArray[i].collision){ 
        // byte radius = -20*sin(200 * (millis()-schedulerTime) / 50000.0)+80 ;
         //midiNoteOn(0, radius, 127);
        midiCollision(i, spotArray[i].target_angle);
        //midiCollisionPercussion(i);
        // midiTouchWall(i);  
    }
      
      if(spotArray[i].behaviourMode == COUNTOUR_FOLLOWING){ 
        midiPitchAngle(i, spotArray[i].onContour, spotArray[i].target_angle);
        //midiPitchBendAngle(i, spotArray[i].onContour, spotArray[i].target_angle);

        // change size?:
       // if (i==1) {
       // byte radius = 5*sin(200 * (millis()-schedulerTime) / 10000.0)+35;;//15*sin(200 * (millis()-schedulerTime) / 10000.0)+45;
       // spotArray[1].setRadiusSaccade( radius );
       //  if (radius>55) midiCollisionPercussion(i);//midiCollision(i, 0);//
       // }
      }
      
     
     // send coordinates of spot (regardless of spot number) on the SERIAL port:  
     if (sendSerial==true) {
     Serial.print(int(spotArray[i].getX()), DEC);  // rem: getX gives a float between 0 and 4000
     Serial.print('X'); 
     Serial.print(int(spotArray[i].getY()), DEC);
     Serial.print('Y'); 
     Serial.print(spotArray[i].identifier);
     Serial.print('I'); // for identifier (this will enable setting the coordinates for the proper spot
     // on the processing program).
     }
       
    }    
  }
  
  scheduler();
}

// POTENTIOMETER:
void potentiometerThresholdRead() {
 potValue=analogRead(pinAnalogPot); // value between 0 and 1023
 // set the corresponding potentiometer based fixed threshold:
 potFixThreshold=1.0*potValue/1023*255; // value between 0 and 255 (the readout of the custom lockin is 8 bytes)
 //Serial.println(potFixThreshold);
}




